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Introduction 
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1.  Introduction 

An  important  goal  of  research  on  programming  languages  for  computer  controlled  manipulators 
is  a  language  in  which  assembly  operations  can  be  concisely  described.  Two  major  approaches  to 
manipulator  programming  have  been  identified  [34]: 

1.  Kxplicit  programming  —  in  which  the  user  specifics  all  tire  manipulator  motions  needed  to 
accomplish  a  task. 

2.  Model-based  programming  —  in  which  the  user  specifics  gcomciric  models  of  parts  and  a 
description  of  the  task  in  terms  of  these  models.  The  detailed  manipulator  motions  arc  derived 
by  die  assembly  system  from  diese  specifications. 

This  paper  presents  algorithms  for  some  of  the  central  gcomciric  problems  that  arise  in  die  model- 
based  approach  to  manipulator  programming.  In  particular,  this  paper  deals  with  the  class  of 
problems  that  involve  finding  where  to  place  or  how  to  move  a  solid  object  in  the  presence  of 
obstacles.  ITic  solution  to  this  class  of  problems  is  essential  to  the  automatic  planning  of  manipulator 
transfer  movements,  i.c.  the  motions  to  grasp  a  part  and  place  it  at  some  destination.  For  example, 
planning  transfer  movements  requires  the  ability  to  plan  padis  for  die  manipulator  dial  avoid  colli¬ 
sions  with  objects  in  the  workspace  and  die  ability  to  choose  safe  grasp  points  on  objects.  The  ap¬ 
proach  to  these  problems  described  here  is  based  on  a  method  of  computing  an  explicit  representation 
of  the  manipulator  configurations  that  would  bring  about  a  collision  [27], 

Several  model-based  manipulator  systems  have  been  described  in  the  recent  literature:  AI.  [10] 
[46],  Autopass  [24]  [28]  [50],  I.AMA  [25]  [26]  and  RAP1  [38]  [39]  [40],  Iliesc  arc  experimental  systems, 
currently  under  development1  .  Work  on  die  model-based  aspects  of  AI.  has  focused  on  techniques 
for  making  coding  decisions  in  manipulator  programs.  Hie  decisions  arc  made  among  a  fixed  set  of 
strategics  so  as  to  minimi/.c  estimated  execution  times  and  so  as  to  bring  estimates  on  the  accuracy 
of  part  positions  within  specified  bounds.  A  central  technical  issue  in  this  approach  is  deriving  the 
accuracy  estimates  from  geometric  relationships  and  local  accuracy  information.  RAPT  has  focused 
on  the  specification  of  manipulator  programs  by  specifying  the  desired  symbolic  spatial  relation¬ 
ships  among  objects.  These  relations  arc  then  translated  into  algebraic  constraints  on  the  position 


'The  AL  language,  as  originally  described,  includes  explicit  as  well  as  model-based  programming  capabilities.  The  former 
arc  currently  available,  while  (he  latter  arc  still  in  the  experimental  stage. 
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The  ‘Tick  and  Place"  Synthesis  Problem 


parameters  of  the  objects,  which  can  be  solved  by  symbolic  manipulation.  These  algebraic  solution 
techniques  arc  also  used  to  complete  the  specification  of  partially  specified  actions  so  as  to  achieve 
the  desired  relationships.  Implementation  work  on  KAMA  and  Aulopass  has  focused  on  techniques 
for  planning  collision-free  motions,  c.g.  grasping  and  parts  transfer  motions,  using  polyhedral  object 
models.  The  techniques  reported  in  this  paper  arc  extensions  of  the  Autopass  obstacle  avoidance 
algorithm  and  KAMA'S  grasping  strategies. 

A  number  of  important  problems  relevant  to  model-based  manipulator  programming  have  been 
addressed  independently  of  any  manipulator  system,  for  example  the  problem  of  specifying  com¬ 
pliant  motion  strategics  based  on  geometric  and  kinematic  models  of  a  uisk  (30],  tire  selection  of 
grasping  positions  (5]  (31]  (35]  (51],  and  the  problem  of  collision  detection  and  collision  avoidance 
among  obstacles  [3]  (7]  (12]  (33]  (47]. 

The  algorithms  discussed  in  this  paper  arc  based  on  previous  work  on  obstacle  avoidance  algo¬ 
rithms.  In  particular.  [48]  [49]  first  formulated  the  obstacle  avoidance  problem  in  terms  of  an  obstacle 
transformation  which  allows  treating  die  moving  object  as  a  point.  A  similar  transformation  was  also 
used  in  [1]  (2|  [4]  (45]  for  the  template  layout  problem;  related  applications  arc  also  discussed  in  [11] 
[16].  Generalizations  of  these  obstacle  transformation  techniques  and  a  review  of  related  work  can  be 
found  in  [27|  and  (28],  Other  approaches  to  automatic  obstacle  avoidance  arc  reviewed  in  (23]  (48], 


2.  Tlte  "Pick  and  Place”  Synthesis  Problem 

The  most  common  transfer  movements  arc  of  the  "pick  and  place"  type,  consisting  of  (1)  moving 
the  manipulator  from  its  current  configuration2  to  a  grasp  configuration  on  some  object,  P,  (2)  grasp¬ 
ing  P,  and  (3)  moving  P  to  some  specified  configuration.  The  "pick  and  place"  synthesis  problem  is 
that  of  deriving  the  manipulator  motions  that  will  carry  out  a  "pick  and  place"  transfer  movement, 
given  as  input  the  following  data; 

1 .  a  geometric  description  of  the  manipulator  and  the  objects  in  the  workspace, 

2.  the  current  configurations  of  the  manipulator  and  the  objects  in  the  workspace, 


^Configuration  will  be  used  here  to  refer  to  the  combined  position  and  orientation  of  an  object  as  well  as  to  the  set 
of  joint  parameters  specifying  the  arrangement  of  manipulator  links. 
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3.  the  desired  final  configuration  of  P,  and 

4.  (Optional)  the  grasp  configuration  on  P. 

This  paper  focuses  on  the  geometric  aspects  of  the  "pick  and  place"  synthesis  problem.  For 
example,  when  the  grasp  configuration  is  known,  the  "pick  and  place"  synthesis  problem  is  equivalent 
to  finding  collision-free  paths  for  the  manipulator  and  P  between  the  configurations  in  items  2,  3 
and  4  above;  when  the  grasp  configuration  is  unknown,  there  is  the  additional  task  of  choosing  a 
configuration  such  that: 

1 .  the  manipulator's  fingers  arc  in  contact  with  P, 

1  the  manipulator  docs  not  collide  with  nearby  objects, 

3.  the  configuration  is  reachable,  and 

4.  the  object  is  stable  in  the  manipulator's  hand. 

The  first  three  conditions  reflect  geometric  constraints  on  the  manipulator  configuration,  relative  to  P 
and  to  other  objects  in  the  workspace.  The  stability  condition  reflects  aspects  of  grasping  beyond  the 
purely  geometric,  but  when  P  is  small  relative  to  the  manipulator  hand  and  when  parts  mating  effects 
arc  ignored,  then  stability  considerations  can  typically  be  reduced  to  geometric  heuristics  (see  Section 
9.6). 

The  geometric  aspects  of  "pick  and  place"  can  be  formulated  in  terms  of  two  fundamental  spatial 
planning  problems  [27],  Findspacc  and  Findpath,  which  occur  in  many  applications.  The  definition  of 
these  basic  problems  arc  presented  below  for  the  ease  of  polyhedral  objects. 

l.ctP  be  a  convex  polyhedron  that  bounds  the  workspace  and  which  contains  kg  other,  possibly 
overlapping,  convex  polyhedra  Bj  designated  as  obstacles.  I.ct  A,  the  object  being  moved,  be  the 
union  of  h*  convex  polyhedra  /!»,  i.e.  A  =  Ut^=i 

1.  Findspacc  —  Find  a  configuration  for  A,  inside  R,  such  that  Vt'Vj :  ft  Bj  =  0.  This  is 

called  a  sqfe configuration. 

2.  Findpath  —  Find  a  path  for  A  from  configuration  s  to  configuration  g  such  that  A  is  always 

in  R  and  A  never  overlaps  any  of  the  By  Ibis  is  called  a  sajc  path. 
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Clearly,  "pick  and  place"  with  known  grasp  configuration  can  be  viewed  as  a  sequence  of  two 
Findpath  problems.  In  addition,  the  configurations  which  are  legal  candidates  for  grasping  can  be 
derived  froin  solutions  to  the  Findspacc  problem. 

'Hie  reduction  of  the  "pick  and  place"  problem  to  these  more  fundamental  geometric  problems 
assumes  that  the  locations  of  all  objects  arc  known  to  high  accuracy  and  that  the  path  of  the 
manipulator  can  be  controlled  to  the  same  precision.  In  a  realistic  environment,  there  is  always  uncer¬ 
tainty  in  die  positions  of  objects  and  error  in  die  control  of  die  manipulator.  Section  10  discusses  the 
effects  of  uncertainty. 


3.  The  C apace  Approach  to  Spatial  Planning:  Overview 

In  this  section,  an  overview  of  the  Configuration  Space  approach  to  spatial  planning  will  be 
presented;  further  details  can  be  found  in  [27]. 

Ihc  position  and  orientation  of  a  rigid  solid  can  be  specified  by  a  single  6-dimcnsional  vector, 
called  its  configuration.  Ihc  6-dimcnsional  space  of  configurations  for  a  solid,  A ,  is  called  its 
Configuration  Space  and  denoted  Cspace.y  For  example,  a  configuration  may  have  one  coordinate 
value  for  each  of  die  x,  y,  z  coordinates  of  a  selected  point  on  the  object  and  one  coordinate  value  for 
each  of  the  object's  Ruler  angles  [21].  In  general,  an  n-dimcnsional  configuration  space  can  be  used 
to  model  any  system  for  which  the  position  of  every  point  on  die  objcct(s)  can  be  specified  with  n 
parameters.  An  example  is  the  configuration  of  an  industrial  robot  with  n  joints,  where  n  is  typically  5 
or  6.  In  CapaccA,  the  set  of  configurations  of  A  where  A  overlaps  B,  i.e.  A  D  B  ^  0,  will  be  denoted 
COa(B).  the  CapaceA  Obstacle  due  to  B.  Similarly,  those  configurations  of  A  where  A  is  completely 
inside  B.  i.e.  A  C  B.  will  be  denoted  CIa(B),  the  C  apace  a  Interior  of  B.  Together,  these  two 
CapaceA  constructs  embody  all  the  information  needed  to  solve  Findspacc  and  Findpath  problems. 
Note  that  CIa(B)  =  — COa(—B ),  where  —X  denotes  the  set  complement  of  X  in  R. 


3. 1 .  Fixed  Orientation  of  A 


In  two  dimensions,  if  the  orientation  of  a  convex  polygon  A  is  fixed,  CapaceA  is  simply  the  (x,  y) 
plane.  ITtis  is  so  because  the  (x,  y)  position  of  some  reference  vertex  rvA  is  sufficient  to  specify  the 
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Figure  1.  The  Ch]ki<ta  obstacle  due  lo  /(,  for  fixed  orientation  of  A. 

polygon's  configuration.  In  this  case,  the  presence  of  anotoct  convex  polygon  B  constrains  rvA  to 
be  outside  of  COA(B),  a  larger  convex  polygon,  shown  as  tlic  shaded  region  in  Figure  1.  Since 
CO  \(B),  in  this  case,  is  a  set  of  (x,  y)  values,  it  is  denoted  COA"(B).  Similarly,  if  A  and  B  arc  three- 
dimensional  polyhedra  in  fixed  orientations,  then  the  C space  obstacles  arc  denoted  COxJ'z(B).  Thus, 
the  Findspace  problem  for  polygons,  with  fixed  orientation,  can  be  transformed  to  the  equivalent 
problem  of  placing  rvA  outside  ofCOA!/(B),  but  inside  CIZ^{R).  Similarly,  for  multiple  obstacles#;, 
a  location  for  A  is  safe  if  and  only  if  rvA  is  not  inside  any  of  the  CO\v(Bj),  but  is  inside  CIAV(R). 

If  the  orientation  of  A  is  fixed,  then  the  Findpath  problem  for  the  polygon  A  among  the  Bj  is 
equivalent  to  the  Findpath  problem  for  the  point  rvA  among  the  COT^/{BJ).  When  tlic  CO\y(Bj) 
arc  polygons,  tlic  shortest1  safe  paths  for  ru\  arc  piecewise  linear  paths  connecting  tlic  start  and  the 
goal  via  the  vertices  of  the  COAv[B)  polygons.  Figure  2.  Therefore,  Findpath  can  be  formulated  as  a 
graph  search  problem.  The  graph  is  formed  by  connecting  all  pairs  of  vertices  of  CspaceA  obstacles 
(and  the  start  and  goal)  that  can  "see"  each  other,  i.e.  can  be  connected  by  a  straight  line  that  does 
not  intersect  any  of  the  obstacles.  The  shortest  path  from  the  start  to  the  goal  in  this  visibility  graph 


^This  assumes  Euclidean  distance  as  a  metric.  For  the  optimality  conditions  using  a  rectilinear  (Manhattan)  metric  see 
[22]. 
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l'i(*urc  2.  This  figure  illustrates  the  1‘indpath  problem  and  its  formulation  using  Cs pace  obstacles.  Note 
that  the  shortest  collision- free  paths  amnccl  the  origin  and  the  destination  via  the  scrticcs  of  the  (’spact* 
obstacles 


(Vgrapli)  is  tlic  shortest  safe  path  for  A  among  the  Bj  [28],  This  algorithm  solves  Findpath  problems 
when  the  orientation  of  A  is  fixed.  Hut.  because  they  require  moving  A  along  obstacle  boundaries, 
shortest  paths  arc  very  susceptible  to  inaccuracies  in  the  object  models. 

'ITtc  approach  to  I’indspaec  and  Findpath  described  above  generalizes  to  problems  involving 
three  dimensional  polyhcdra  with  fixed  orientation.  The  generalization  requires  the  use  of  a  three- 
dimensional  Cspace 4,  representing  the  space  of  ( x,y,z )  positions  of  rv\.  In  this  Cspace,  the 
obstacles  arc  also  polyhcdra,  denoted  COI/z(B).  However,  the  Vgraph  algorithm  has  several  addi¬ 
tional  drawbacks  when  die  obstacles  arc  diree-dimcnsional: 

1.  Shortest  paths  do  not  typically  traverse  the  vertices  of  the  CO^l(Bj). 

2.  There  may  be  no  paths  via  vertices,  within  die  enclosing  polyhedral  region  R,  aldiough 

other  types  of  safe  paths  within  R  may  exist. 

These  drawbacks  may  be  alleviated  by  introducing  additional  nodes  in  the  Vgraph  which  do  not 
correspond  to  vertices  128],  An  alternative  strategy  for  finding  safe  paths  among  two-  or  three- 
dimensional  Cspace a  obstacles  is  discussed  in  Section  7. 
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3.2.  Algoriilinn for  COI^J'[B) 

The  central  operation  in  the  Cspace  approach  to  Findspace  and  I  indpath  in  two  and  three 
dimensions  is  computing  C(Vf(B)  and  COJf'(B)  respectively.  If  A  and  B  arc  convex  polyhedra,  it  is 
simple  to  show  [27]  that 


COTfz[B)  =  flQ  Mo  =  conv(vert{B)  ©  vert{[A)n)) 

where  conv(X)  is  the  convex  hull  of  X  [14],  vert(X)  is  die  set  of  vertices  of  the  polyhedron  X, 
X  QY  =  {x  —  y  |  i  G  X  and  y  £  Y}  and  (Y)n  means  the  polyhedron  X  in  its  initial 
configurations,  where  rv\  is  at  the  origin.  This  result  and  the  existence  of  0(n  log  n)  convex  hull 
algorithms  for  finite  sets  of  points  in  5R 1  [41],  lead  directly  to  an  0(v2  log  v)  algorithm  for  C0^2{B), 
where  v  =  |uerf(<4)|  +  \vert(B)\.  The  result  also  holds  when  A  and  B  arc  convex  polygons,  but  more 
efficient  algorithms  exist  for  this  ease.  In  particular,  an  0(v)  algorithm  for  COrf[B)  is  described  in 
[271. 


3. 3.  Variable  Orientation  of  A 

When  A  is  a  three-dimensional  solid  which  is  allowed  to  rotate,  CO.\(B)  is  a  complicated  curved 
object  in  a  six-dimensional  Cspace  \.  Rather  than  compute  these  objects  directly,  die  approach  taken 
here  is  to  use  a  sequence  of  two-  and  three-dimensional  objects  to  approximate  the  high-dimensional 
CspaceA  obstacles.  In  particular,  a  six-dimensional  CspaceA  obstacle  for  a  rigid  solid  can  be  ap¬ 
proximated  by  projections  of  its  ( x ,  y,  a)-slices.  A  j'-slice  of  an  object  C  £  Si"  is  defined  to  be: 


Where  t j  and  arc  the  lower  and  upper  bounds  of  the  slice,  respectively.  Then,  if  K  is  a  set  of 
indices  between  1  and  n,  a  /C-slicc  is  the  intersection  of  all  the  j  slices  for  j  £  K.  Notice  that 
a  /©slice  of  C  is  an  object  of  the  same  dimension  as  C.  Slices  can  dicn  be  projected  onto  those 
coordinates  not  in  K  to  obtain  objects  of  lower  dimension. 

Figure  3  shows  a  two-dimensional  example  of  slice  projection.  The  objects  shown  shaded  repre¬ 
sent  the  (x,  y)  projection  of  three  0-slices  of  CO.\(B)  when  A  and  B  arc  convex  polygons.  These  slices 
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I*'i|>iirc  .V  Slice  projections  of  C a  pan  a  obstacles  computed  using  the  (c,  t/)-arca  swept  out  by  1  over  a 
range  of  0  values  I  jell  of  the  shaded  obstacles  is  the  (a ,  y) -projection  of  a  0  slice  of  <  '0A{li)  Ihe  figure 
also  shows  a  polygonal  approximation  to  the  slice  projection  and  the  polygonal  approximation  to  ihe  swept 
volume  from  which  it  derives 


represent  configurations  where  A  overlaps  B  for  some  orientation  of  A  in  the  specified  range  of  0. 
In  (27)  is  a  proof  that  these  slice  projections  are  equivalent  to  live  COTU  of  the  area  (volume)  swept 
out  by  A  over  the  range  of  orientations  of  the  slice.  Note  that  approximating  live  swept  volume  as  a 
polyhedron  leads  to  a  polyhedral  approximation  for  the  projected  slices  of  the  C space A  obstacles,  as 
shown  in  Figure  3. 


Slice  projection  has  two  important  properties: 

I.  A  solution  to  a  Findspacc  problem  in  any  of  the  slices  is  a  solution  to  the  original  problem, 
but  since  the  slices  arc  an  approximation  to  the  CspaceA  obstacle,  the  converse  is  not  ncccs- 
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figure  4.  An  illustration  or  the  I  indpath  algorithm  using  slice  projection  described  by  l  orano  Perez  and 
Wesley  in  (28|.  A  number  of  slice  projections  of  the  (up rm  obstacles  are  constructed  Tor  dilTerent  ranges 
of  orientations  of  A  The  problem  of  planning  safe  paths  in  the  high  dimensional  Capa&A  is  decomposed 
into  (I)  planning  safe  paths  via  CO  vertices  within  each  slire  projection  and  (2)  moving  between  slices,  at 
configurations  that  are  safe  in  both  slices  t|  represents  t  in  its  initial  configuration.  -1  j  represents  A  in  its 
final  configuration  and  t  j  is  a  simple  polyhedral  approx  miat  ion  to  the  swept  volume  of  A  between  its  initial 
and  final  orientation. 


2.  The  siicc  projection  of  a  Cspace, \  obstacle  can  be  computed,  by  using  the  swept  volume 

operation,  without  having  to  compute  the  high-dimensional  CspaceA  obstacle,  see  Section  5. 

When  rotations  of  A  arc  allowed,  the  slice  projection  operation  can  be  used  to  extend  the  Vgraph 
algorithm  described  earlier  to  find  safe  (but  sub-optimal)  paths  [28],  A  number  of  slice  projections 
of  the  Cspact,\  obstacles  arc  constructed  for  different  ranges  of  orientations  of  A.  The  problem  of 
planning  safe  paths  in  the  high-dimensional  C space, \  is  decomposed  into: 

1.  planning  safe  paths  via  the  vertices  of  Cspace,\  obstacles  within  each  slice  projection,  and 

2.  moving  between  slices,  at  configurations  that  arc  safe  in  both  slices. 
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l  iljiirc  5.  Models  of  objects  are  structured  as  trees  of  convex  polyhedra:  internal  nodes  represent  the  union 
of  their  suit-trees  l  inked  polvhcdra  arc  used  to  represent  manipulators:  internal  nodes  represent  joints  and 
the  leases  represent  links  lltc  nesting  of  sub  trees  in  the  models  of  linked  poljhcdra  reflect  the  cascading 
effect  of  joint  motions. 


Both  of  these  types  of  motions  can  be  modelled  as  links  in  the  Vgraph,  therefore  the  complete  algo¬ 
rithm  can  be  formulated  as  a  graph  search  problem,  litis  approach  is  illustrated  in  Figure  4.  However, 
since  the  obstacles  are  three-dimensional,  the  Vgraph  algorithm  is  subject  to  the  drawbacks  described 
earlier. 


4.  Findpath  for  Cartesian  Manipulators 

This  section  overviews  an  implementation4  of  the  Findpath  algorithm,  for  cartesian  manipulators 
(see  definition  below).  Sections  5  through  8  present  a  more  detailed  description  of  the  implementation. 
'Hie  system  inputs  are: 

'’The  current  implementation  is  written  in  I.ISP  for  the  MIT  MSP  Machines. 


L 
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1.  A  polyhedral  model  of  the  workspace  —  where  each  object  is  represented  by  a  tree  of 
convex  polyhedra,  sec  Figure  5(a). 

2.  A  polyhedral  model  of  the  manipulator  —  represented  as  a  set  of  link  bodies  connected  by 
rotary  or  prismatic  joints,  sec  Figure  5(b). 

3.  A  kinematic  model  of  the  manipulator  —  currently,  partly  embedded  in  proccdurciwhich 
apply  to  die  polyhedral  model  and  partly  in  the  model  structure. 

4.  A  start  and  a  goal  configuration  for  the  manipulator. 

The  system  output  is  a  safe  path  from  the  start  to  the  goal  configurations  of  the  manipulator.  The 
paths  are  composed  of  a  sequence  of  linear  segments  in  the  C space  of  the  manipulator. 

The  implementation  described  here  is  limited  to  cartesian  manipulators,  i.c.  those  having  three 


12 


Computing  the  ( 'space  A  Obstacles 


perpendicular  translational  degrees  of  freedom  corresponding  to  the  x.  y  and  z  axes  and  up  to  three 
rotary  degrees  of  freedom,  usually  centered  at  the  wrist,  l-igurc  6  illustrates  two  different  types  of 
cartesian  manipulators.  The  restriction  to  cartesian  manipulators  allows  the  use  of  the  CO^,z(B) 
algorithm  described  in  Section  3.2  as  the  main  tool  for  capturing  path  constraints. 

The  F'indpath  algorithm  carries  out  the  following  steps  in  turn: 

1.  Constructing  the  C space  A  obstacles  —  The  slice  projections  of  the  C space A  obstacles 
approximate  the  constraints  on  the  configurations  of  the  manipulator  due  to  the  presence  of 
objects  in  the  manipulator’s  workspace,  see  Section  5. 

2.  Representing  free  space  —  Once  the  CspaceA  obstacles  are  known,  the  system  computes 
a  decomposition  of  the  space  outside  these  obstacles  into  convex  polyhedral  cells;  these 
polyhedra  arc  then  linked  into  a  graph  structure  called  the  Free  Space  Graph.  Rach  node  of  the 
graph  represents  a  free  space  cell  and  a  link  between  cells  indicates  that  they  touch  or  overlap, 
see  Section  6. 

3.  Searching  for  a  safe  path  —  The  Free  Space  Graph  is  searched  to  locate  a  cell  path,  a 
connected  set  of  free  space  cells  that  join  the  origin  and  the  destination.  From  the  cell  path,  the 
system  derives  a  line  path,  a  piecewise  linear  path  in  the  manipulator’s  C space,  sec  Section  7. 


5.  Computing  the  Cspace,\  Obstacles 

The  first  and  most  important  step  in  the  F'indpath  algorithm  is  that  of  computing  the  CspaceA 
obstacles  arising  from  the  presence  of  objects  in  die  workspace.  The  CspaceA  currently  used  by  the 
system  is  the  seven  dimensional  joint  space  of  the  manipulator,  i.e.  x,  y,  and  z  displacements,  the 
three  wrist  rotations  and  the  finger  opening.  The  CspaceA  obstacles  arc  complicated  objects  in  this 
high-dimension  space.  To  avoid  having  to  deal  directly  with  these  objects,  die  system  makes  use  of 
slice  projection  to  approximate  the  CspaceA  obstacles  by  a  set  of  three-dimensional  obstacles. 

Ihe  COAyz(B)  algorithm  of  Section  3.2  computes  an  [x,  y,z)  cross-section  of  COA(B)  for  a 
specified  orientation  of  A.  But,  this  algorithm  can  be  adapted  to  compute  die  ( x ,  y,  z)-slicc  projections 
of  CO,\{B).  The  construct  that  i  elates  slice  projections  to  the  cross-sections  is  the  swept  volume  of 
an  object.  The  swept  volume  of  4  is  the  union  of  (/4)0,  i.e.  A  in  configuration  o,  for  a  within  the 
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configuration  range  denoted  by  |c,c']/x,  where  c  and  d  arc  configurations  of  A  and  K  is  a  subset  of 
die  configuration  parameters.  A  configuration  a  is  in  the  range  |c,  if,  for  each  i  in  K ,  the  ith 
parameter  of  a  is  between  the  i,h  parameters  of  c  and  d.  For  example,  if  c  and  d  are  of  the  form 
(A,  fit)  and  K  =  {3}.  dicn  the  swept  volume  of  A  over  the  range  [c,  d]^  refers  to  die  union  of 
A  over  <i  set  of  configurations  differing  only  on  (3j.  The  swept  volume  of  A  over  this  configuration 
range  is  denoted  A \c,  d]/s .  It  can  be  shown  (27J  that  the  ( x ,  y,  ,2) -si ice  projection  of  COA[B)  over  the 
orientation  range  contained  in  [c,  d]h  is  the  same  as  CO\  UM 

I11  summary,  the  computational  requirements  of  the  slice  projection  technique  are: 

1.  Choosing  a  decomposition  of  the  orientation  ranges  of  die  cartesian  manipulator  into  sub¬ 
ranges,  [c,  d]h ,  to  be  used  for  slice  projection. 

2.  Computing  polyhedral  approximations  to  A\c,  d]h  for  each  orientation  range. 

3.  Computing  CO1^ ^k(Bj)  for  each  obstacle  flj  and  each  orientation  range. 

Ihis  section  addresses  these  issues.  First  we  assume  that  the  orientation  ranges  defining  the  slices  are 
given;  die  discussion  of  choosing  slice  parameters  will  be  taken  up  at  the  end  of  the  section. 

5. 1.  Computing  the  Swept  Volume  for  Linked  Polyhedra 

The  swept  volume  of  a  polyhedron  A  over  a  range  of  translations  is  another  polyhedron.  Let 
T  C  /  be  the  set  of  configuration  parameters  corresponding  to  the  translations  of  A.  If  A  is  a  convex 
polyhedron  and  the  range  of  positions  of  the  reference  vertex  of  A  over  the  range  of  translations 
[c,cV  can  be  represented  as  a  convex  polyhedron  V,  then  A\c,d\r  =  A  0  V  where  X  ®  Y  = 
{  z  y  |  x  G  X  and  y  E  Y  }.  Since  A  ®  V  =  conv(vert(A)  ©  vert{V))%  this  leads  to  a  direct 
algorithm  for  computing  the  swept  volume  for  translation.  If  the  range  of  configurations  includes 
rotations  then  the  swept  volume  is  not  a  polyhedron.  In  the  rest  of  the  paper  it  is  assumed  that 
a  polyhedral  approximation  to  the  swept  volume  is  always  available.  'Hie  Appendix  describes  an 
algoridim  to  compute  a  simple  approximation  to  the  swept  volume  of  a  convex  polyhedron  under 
pure  rotation. 

The  swept  volume  of  A,  a  rigid  object,  resembles  another  rigid  object  with  the  same  number 
of  degrees  of  freedom.  But  for  manipulators,  modelled  as  linked  polyhedra,  the  situation  is  more 
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I'iKurc  7.  Changes  in  the  second  joint  angle  from  0t  to  fft  causes  changes  in  the  configurations  of  both  link 
A  2  and  link  A  3. 


complex.  I  . inked  polyhedra  arc  kinematic  chains  with  polyhedral  links  and  prismatic  or  rotary  joints. 
The  relative  position  and  orientation  of  adjacent  links.  A,  and  Al+l,  is  dcicnnincd  by  lire  ilh  joint 
parameter  (angle)  [36],  The  set  of  joint  parameters  of  a  linked  polyhedron  completely  specifies  the 
position  and  orientation  of  all  the  links. 

Note  that  for  a  linked  polyhedron,  the  position  of  link  j  typically  depends  on  the  positions  of 
links  k  <  j,  which  arc  closer  to  the  base  than  link  j.  Let  K  =  {j},  c  =  (0,),  d  =  ( 0' ).  and  [c,d]K 
define  a  range  of  configurations  differing  on  the  j'h  Cspace  (  parameter.  Since  joint  j  varies  over  a 
range  of  values,  links  l  >  j  will  move  over  a  range  of  positions  which  depend  on  the  values  of  c  and 
d.  as  shown  in  Figure  7.  The  union  of  each  of  the  link  volumes  over  its  specified  range  of  positions  is 
the  swept  volume  of  the  linked  polyhedron.  The  swept  volume  of  links  j  through  n  can  be  taken  as 
defining  a  new  jlh  link.  The  fust  j  —  l  links  and  the  new  j111  link  define  a  new  manipulator  whose 
configuration  can  be  described  by  the  first  j  —  1  joint  parameters.  On  the  other  hand,  die  shape  of  the 
new  link  j  depends  not  only  on  the  /(-parameters  of  c  and  d.  i.c.  0,  and  0f,  but  also  on  ft  for  l  >  j. 
This  implicit  dependence  on  parameters  of  c  and  d  that  arc  not  in  K  is  undesirable,  since  it  means 
that  the  shape  of  the  new  jlh  link  will  vary.  I. citing  K  =  {j, .  .  . ,  n},  then  the  shape  of  the  swept 
volume  depends  only  on  the  /(-parameters  of  c  and  d,  while  its  configuration  is  determined  by  the 
(I  —  /()-paramctcrs.  A  swept  volume  that  satisfies  this  property  is  called  displaceable. 
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Figure  8.  Computing  the  swept  volume  Tor  linked  poljhcdra.  If  [c,  involves  ranges  of  configurations  of 

the  second  and  third  link,  first  compute  (he  swept  volume  for  the  (hud  link  and  then  the  swept  volume  for 
the  union  of  the  second  link  and  (lie  swept  volume  of  the  third  link. 


Given  an  operation  for  computing  (a  polyhedral  approximation  to)  the  swept  volume  of  a 
polyhedron,  see  Appendix,  then  this  operation  is  applied  to  computing  the  swept  volume  oflinkcd 
polyhedra.  The  swept  volume  A [c,  c?]K  is  computed  by  die  following  process,  illustrated  in  Figure  8: 

1.  Let  i  =  n,  where  n  >  0  is  the  number  of  links  in  die  linked  polyhedra,  |/|  =  n;  let 

2.  Place  A  in  configuration  c; 

3.  Let  A*  = 

4.  If  *  G  K  then  let  A*  =  A*[c,  </]<,},  i.e.  update  A *  to  be  the  swept  volume  of  A *  over  the 
range  of  ilh  joint; 


5.  Let  i  =  t  —  1.  If  *  =  0  then  stop,  else  go  to  step  3. 
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The  swept  volume  obtained  in  this  fashion  can  then  be  used  to  compute  die 


5.2.  Computing  slier  projections  for  C  apace A  obstacles 

If  /t[c,  c'j/v  overlaps  some  obstacle  B  tlien.  for  some  configuration  o  in  the  range  [c,  d\n,  (/!)„ 
overlaps  B.  Ihe  converse  is  also  true.  If  /I [c,  o'] /x  is  displaceable,  then  CO,^c  ^h(B)  is  the  set  of 
I  —  K  projections  of  those  configurations  of  A  within  [c,  for  which  A  overlaps  B.  Kquivalcntly, 
CP-tjr.r'iK  {&)  is  the  /  —  K  projection  of  die  [c,  c'J^  slice  of  COA(B).  \TA  is  a  cartesian  manipulator 
and  K  is  the  index  set  for  the  wrist  rotations  of  die  manipulator,  then  the  configurations  of  the  swept 
volume  arc  the  (x,  y,  z)  positions  of  some  point  on  the  manipulator.  The  algorithm  of  section  3.2  can 
be  used  to  compute  ^  (0)  and  thereby  compute  the  required  slice  projections  of  COA(B). 

Given  the  swept  volume  of  die  manipulator  model  for  a  particular  range  of  parameters  [c,  </]/<-, 
the  next  step  is  to  compute  the  slices  of  all  die  C space  obstacles  for  the  manipulator  over  that  range 
of  configurations:  this  set  is  denoted  COS[c,  c1].  In  previous  discussions  of  the  CO”,:(B)  algorithm 
we  have  assumed  that  A  and  B  were  single  polyhedra;  we  saw  in  the  previous  sections  diat  both 
die  object  and  manipulator  models  arc  structured  as  part  trees,  whose  leaves  arc  convex  polyhedra. 
The  actual  model  of  a  manipulator  or  a  part  is  die  union  of  die  fringe,  i.c.  die  set  of  leaves,  of  the 
corresponding  part  tree.  Thus  if  >4  =  U*=i  Ai  and  B  =  (J*!L  ,  Bj.  die  following  result  can  be  used 
in  computing  COAJ*(B): 

coA(B)  =  U  U  c°M 

«=i  i=i 

This  rcsidt  means  that  kA  X  kit  applications  of  die  COff*(Bj)  algorithm  must  be  carried  out  to 
compute  CO%'z(B)  exactly.  In  die  "pick  and  place"  application,  an  exact  model  of  all  the  C space 
obstacles  is  not  usually  needed  since  the  manipulator  will  not  move  close  enough  to  all  die  obstacles. 

The  amount  of  time  needed  to  compute  the  COS  can  be  reduced  by  simplifying  the  geometric 
models  of  bodi  the  Ai  and  die  B}  when  appropriate.  The  current  implementation  uses  a  simple  family 
of  succcsivcly  finer  approximations  for  objects  based  on  the  part  tree.  Consider  the  part  tree  for 
an  object  Bj,  where  each  of  the  leaves  of  the  tree  is  a  convex  polyhedron.  Define  a  covering  node 
set  recursively  to  be  cidicr  (1)  the  set  containing  just  the  root  of  the  part  tree  or  (2)  obtained  from 
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.mother  covering  tuxlc  set  by  replacing  some  node,  internal  to  the  part  tree,  with  all  its  descendants.  If 
each  internal  node  represents  the  union  of  all  its  descendants,  then  every  covering  node  set  is  a  com¬ 
plete  model  of  the  object.  In  practice,  internal  nodes  of  the  part  tree  stoic  the  bounding  rectangular 
solid'  of  the  union  of  all  its  descendants  instead  of  the  union  itself.  Thus,  the  family  of  covering 
node  sees  represents  progressively  more  detailed  models  of  the  part  [29J.  Using  these  approximations 
reduces  the  number  of  applications  of  CO^i:(Dj)  needed  to  compute  the  COS.  since  the  number 
of  poly  lied  ra  in  a  covering  node  set  is  less  than  or  equal  to  that  in  the  full  fringe.  In  addition,  it 
can  be  used  to  simplify  many  of  the  individual  compulations,  because  when  A  and  B  arc  bounding 
rectangular  solids,  computing  CO\,,z(B)  is  trivial.  In  particular,  if  live  bounding  solids  arc  represented 
by  the  endpoints  of  their  main  diagonal,  e.g.  A  =  (01,02)  and  B  =  (61,62).  then  CO^yz(B)  = 
(6,  —  (02—  0|),f>2). 

For  simplicity,  the  current  implementation  uses  a  three  level  part  tree  for  the  swept  volume  of 
the  manipulator  and  for  the  objects  in  the  workspace.  Kach  tree  has  a  root  node  which  models  the 
complete  object  by  one  bounding  rectangular  solid.  The  descendants  of  the  root  arc  bounding  rectan¬ 
gular  solids  for  each  of  the  convex  components  of  the  model  and  the  leaves  of  the  tree  arc  the  convex 
polyhedra  whose  union  is  the  complete  object  model.  Therefore  if  the  object  is  modelled  as  the  union 
of  k  convex  polyhedra,  the  part  tree  has  2k  -f  1  nodes.  Using  this  representation,  CO^/t(B)  can  be 
modelled  as  a  tree  of  similar  structure  with  2(kA  x  kti)  -j-  l  nodes.  Any  covering  node  set  of  this 
tree  is  an  approximation  to  the  Capace.\  obstacle  corresponding  to  B.  In  practice,  the  complete  tree  is 
not  computed  at  once,  rather  the  simplest  approximation,  the  bounding  rectangular  solid  of  the  whole 
object,  is  computed  and  successive  covering  node  sets  are  computed  as  needed.  This  is  discussed 
further  in  section  6. 


5.3.  Choosing  the  Slice  Parameters 

So  far  we  have  assumed  that  the  configuration  ranges  defining  the  Cspace A  slices  were  given  as 
input;  in  this  section,  the  choice  of  ranges  is  discussed.  Ihc  primary  choice  is  how  large  to  make  the 
ranges,  since  it  is  this  that  affects  the  system's  capability  to  use  changes  in  the  orientation  of  the  hand 


'A  bounding  rectangular  solid  for  a  polyhedron  is  a  rectangular  solid  whose  edges  are  parallel  to  the  coordinate  axel 
and  that  completely  includes  the  polyhedron. 
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to  avoid  obstacles.  In  particular: 

t.  The  larger  die  orientation  range  of  a  slice,  the  larger  the  manipulator's  swept  volume,  the 
larger  (and  less  accurate)  the  C  space A  obstacles  and  the  fewer  the  legal  configurations  and  legal 
motions  of  the  manipulator. 

2.  The  smaller  the  orientation  range  of  slices,  the  larger  the  number  of  slices  needed  to  cover 
die  C space  and  the  more  lime  needed  to  compute  die  COS  and  to  search  them  for  a  path. 

These  conflicting  clfccts  can  be  balanced  by  taking  advantage  of  die  fact  that,  for  "pick  and  place” 
motions,  die  accuracy  requirements  are  higher  near  the  start  and  the  goal  of  the  path,  where  the 
manipulator  is  moving  near  obstacles,  than  along  the  rest  of  die  path  [28|  [481  [49J.  This  suggests 
defining  slices  with  small  rotation  ranges  centered  around  die  orientations  of  die  start  and  the  goal; 
slices  with  larger  ranges  may  be  used  for  the  remaining  orientations.  This  approach  is  used  in  the 
current  implementation.  In  particular,  a  COS  is  defined  for  die  orientation  of  the  manipulator  in 
the  start  configuration  and  one  for  die  orientation  manipulator  in  the  goal  configuration;  diese  COS 
correspond  to  slices  with  singular  orientation  ranges,  i.c.  where  the  upper  bound  of  die  range  equals 
the  lower  bound6.  In  addition,  the  total  range  of  parameters  in  C space A  is  divided  among  some 
number  of  other  slices7  each  with  non-singular  ranges.  Furthermore,  slices  with  singular  ranges  are 
defined  for  configurations  at  die  intersection  of  die  slice  parameters  of  die  "larger"  slices.  This  last 
type  of  slice  allows  moving  between  safe  configurations  in  the  "larger”  slices. 

Note  dial  the  computational  burden  of  adding  an  extra  slice  is  very  low  if  bounding  rectangles 
arc  used  for  objects.  Ihis  sacrifices  some  of  die  potential  maneuvering  space,  but  gains  a  very  large 
increase  in  speed.  This  is  the  compromise  taken  in  the  current  implementation. 

Motions  within  a  slice  with  a  singular  orientation  range  are  limited  to  translations,  while  rotation 
is  legal  withm  a  slice  with  non-singular  ranges.  Therefore,  the  classes  of  motions  allowed  by  the 
system  arc  those  composed  of  translations  interspersed  with  rotations,  but  where  the  rotations  happen 
in  increments  defined  by  the  slices  parameters.  Ihis  means  that  this  approach  may  fail  to  find  a  safe 
path  in  situations  where: 

®A  slice  with  a  singular  range  is  the  same  as  a  cross- section. 

’Currently  varying  between  8  and  64. 
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1.  all  safe  paths  require  rotations  combined  with  translations  at  a  finer  resolution  than  that 
allowed  by  the  slice  ranges,  and/or 

2,  the  orientation  ranges  chosen,  although  adequate  in  si/.c,  do  no  match  those  required  in  the 
problem. 

These  problems  can  be  reduced,  at  the  expense  of  more  computation,  by  using  more  slices  with 
smaller  ranges.  But,  there  exists  problems  which  require  continuous  rotation  along  a  path.  In  practice, 
most  robotics  applications  do  not  use  the  very  crowded  environments  that  require  very  high  rotation 
resolution  for  the  "pick  and  place"  motions.  The  reason  for  this  is  that  safe  paths  in  such  environ¬ 
ments  arc  very  hard  for  humans  to  specify,  are  subject  to  positioning  errors  of  the  parts  and  are 
difficult  for  most  industrial  robots  to  execute  reliably  at  medium  or  high  speeds. 

6.  Path  Searching  and  Free  Space 

I  laving  computed  the  Cspace,\  obstacles,  it  still  remains  for  die  system  to  find  a  path  among  these 
obstacles.  This  section  briefly  touches  on  alternative  strategics  for  finding  safe  paths. 

One  approach  to  finding  paths  among  obstacles  is  to  search  for  the  shortest  path  between  the  start 
and  the  goal,  without  considering  other  constraints.  For  example,  the  Vgraph  algorithm  described  in 
Section  3  follows  this  approach.  But,  the  approach  has  some  important  drawbacks.  Shortest  paths  in 
CapaceA  move  along  the  boundaries  of  the  C space A  obstacles  and  arc,  therefore,  very  susceptible 
to  model  inaccuracy  and  position  error.  This  problem  can  be  alleviated  by  adding  a  uniform  "safety 
margin"  around  the  obstacles,  but  doing  so  might  disqualify  some  feasible  paths.  Furthermore, 
no  efficient  algorithms  currently  exist  for  finding  optimal  paths  among  three-dimensional  obstacles. 
Unlike  the  situation  in  two  dimensions,  there  is  no  finite  set  of  points  through  which  shortest  paths 
arc  guaranteed  to  pass.  Hius,  algorithms  would  have  to  be  based  on  iterative  numerical  methods.  For 
these  reasons,  only  heuristic  algorithms  for  finding  safe  paths  will  be  considered  here.  Ihcsc  heuristic 
algorithms  require  less  execution  lime  and  can  be  extended  to  consider  criteria  such  as  safety  margins, 
but  they  will  not  find  the  shortest  path. 

Another  issue  is  whether  the  padi  search  is  conducted  using  primarily  a  representation  of  the 
CspaceA  obstacles  themselves,  as  docs  the  Vgraph  algorithm,  or  of  the  free  space  outside  the 
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obstacles,  as  in  (48]  [49].  Although  these  representations  arc  equivalent,  they  lead  to  different  heuristic 
algorithms.  The  current  implementation  uses  the  free  space  style  of  algorithm  because  it  simplifies 
the  formulation  of  different  search  heuristics,  e.g.  the  use  of  \ariablc  resolution  space  representations 
described  below. 

The  remainder  of  tire  section  deals  with  the  free  space  representation  technique  employed  in  the 
Kindpaih  implementation.  Section  7  discusses  the  path  search  algorithm  used  on  this  representation. 


6. 1.  A  Free  Space  Representation 

The  basic  goals  for  a  space  representation  are  accuracy,  speed  and  compactness.  In  addition, 
it  should  facilitate  heuristics  for  die  task  at  hand,  the  most  important  heuristic  for  a  space  repre¬ 
sentation  is  to  avoid  excess  detail  (and  therefore  time  spent)  on  parts  of  the  space  which  do  not  affect 
die  operation.  Therefore,  the  space  representation  should  not  have  to  maintain  a  perfectly  detailed 
model  everywhere.  Instead,  it  should  have  the  capability  of  maintaining  a  rough  model  and  be  able  to 
selectively  refine  [48]  [49]  subsections  to  be  as  detailed  as  necessary. 

A  number  of  proposals  exist  for  representations  of  space  and  objects  in  space  [9]  [25]  [42];  most 
of  diese  divide  the  space  into  a  set  of  cells.  Hie  proposals  can  be  partially  characterized  along  the 
following  dimensions: 

1.  Shape  uniformity  —  arc  all  cells  equally  shaped? 

2.  Size  uniformity  —  arc  all  cells  the  same  size? 

3.  Orientation  uniformity  —  arc  all  cells  oriented  uniformly? 

4.  Ordering  principle  —  arc  die  cells  ordered  into  an  array,  multi-list,  tree,  or  graph. 

We  will  not  consider  representations  which  use  cells  of  uniform  shape  and/or  size,  since  they  typically 
require  large  numbers  of  cells  to  achieve  sufficient  accuracy8  .  Instead,  we  use  a  hybrid  cell  repre¬ 
sentation  employing  two  types  of  cells;  (1)  rectangular  solids  aligned  with  the  axes  and  (2)  arbitrary 
convex  polyhedra.  The  idea  is  to  use  the  simple  rectangular  cells  away  from  obstacles  where  repre¬ 
sentation  economy  is  important  and  polyhedral  cells  where  high  accuracy,  e.g.  near  an  obstacle,  is 

"lidupa  |IR]  [49]  employed  a  free  space  representation  which  used  rectangular  cells  of  variable  size  This  approach  is 
adequate  tor  motions  that  do  not  closely  approach  the  obstacles. 
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Figure  9.  Ihts  figure  illustrates,  in  two  dimensions,  the  space  representation  employed  in  the  implementation 
of  the  I'indpath  algorithm,  (a)  A  sample  Caprice/*  obstacle  with  its  part  representation,  (b)  The  resulting 
space  representation.  Rectangular  nodes  indicate  mixed  cells,  round  nodes  indicate  full  cells,  and  triangular 
nodes  indicate  empty  cells. 

needed. 

'Hie  space  representation  described  below  is  analogous  to  the  part  representation  described 
earlier,  except  that  a  new  type  of  node  is  introduced.  1110  part  tree  representation  uses  rectangular 
bounding  cells  as  internal  nodes  and  polyhedral  cells  as  leaves.  The  leaves  represent  space  that  is 
FULL,  i.c.  completely  occupied  by  an  object.  The  internal  cells  represent  MIXRD  space,  i.e.  cells 
which  are  part  FULL,  part  BMPrY.  But,  note  that  the  part  tree  docs  not  have  an  explicit  repre¬ 
sentation  of  the  HMPTY  space.  The  space  representation  simply  adds  exp  T  BMPTY  cells  to  the 
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parts  tree  representation.  Then  each  internal  MIXED  node  becomes  the  union  of  its  descendants. 
In  addition,  the  space  representation  introduces  a  new  MIXED  root  node  from  which  all  the  part 
representations  descend. 

Ihc  space  representation  is  built  up  starting  with  a  bounding  rectangular  solid  representing  the 
workspace,  this  is  lire  first  MIXED  cell.  Ihc  descendants  of  tins  node  are  the  MIXED  cells  cor¬ 
responding  to  the  roots  of  the  trees  representing  each  of  the  COTJ''(Bj),  as  described  in  Section  5.2 
and  a  set  of  EMPTY  bounding  rectangular  solids  representing  the  free  space  outside  the  MIXED 
cells.  The  representation  of  each  MIXED  cell  can  be  further  expanded  into  other  EMPTY.  MIXED 
and  EUEI.  cells,  culminating  in  a  representation  imolving  only  EMPTY  and  EUEI.  convex  polyhedral 
cells  as  leaves  of  the  tree  and  MIXED  cells  as  internal  nodes,  Eigurc  9.  The  polyhedral  representation 
of  each  EMPTY  cell  must  be  computed  so  that  it  docs  not  overlap  any  MIXED  or  EUEI.  cells.  As 
with  the  part  representation,  any  covering  node  set  of  this  tree  represents  a  complete  model  of  the 
space,  at  some  non-uniform  resolution.  This  hybrid  cell  representation  is  based  on  a  generalization 
of  the  quad  tree  representation  used  for  images  [8)  117)  1 18)  |20)  [43)  and  the  oct-trcc  representation  of 
objects  (3). 

The  operations  on  the  space  representation  described  above  arc  very  efficient  when  dealing  with 
bounding  rectangular  solids.  The  most  expensive  operation  is  when  the  volume  difference  of  a 
MIXED  rectangular  cell  and  a  l  Ul.l.  polyhedral  cells  must  be  computed0  ;  this  operation  results  in 
a  description  of  the  EMPTY  cells.  However,  this  need  only  be  done  when  high  accuracy  is  required, 
usually  near  the  start  and  the  goal  of  the  path.  Therefore,  the  representation  meets  the  criteria  stated 
at  the  beginning  of  the  section. 


6. 2.  Building  a  Free  Space  Graph 

The  process  described  in  Section  5  produces  a  slice  for  each  CspaceA  obstacle  over  each  of  the 
orientation  ranges,  [eg,  c'Jm  ,  of  the  manipulator's  wrist.  I’lic  set  of  slices  for  all  obstacles  over  one 
orientation  range  is  denoted  COSjc,,  c/l]/<.  Eor  each  of  these  COSi,  a  space  representation  is  com¬ 
puted,  SRi,  as  described  above.  For  each  of  these  SR,,  a  Free  Space  Graph  is  built,  FSG„  this  is 
a  graph  where  each  node  is  an  EMPTY  cell  in  the  SR,  and  a  link  indicates  that  the  cells  touch  or 
The  current  implementation  of  this  operation  uses  repeated  applications  of  a  cutting  and  capping  operation  [6] 
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overlap10  .  In  addition,  it  is  necessary  to  add  links  to  each  FSG,  dial  connect  to  nodes  of  other  FSGj 
whose  rotation  range  overlaps  dial  of  FSGt.  That  is.  for  EMPTY  cells  C,  £  SR,  and  C}  £  SR3,  if 
there  is  some  configuration  c  contained  in  both  cells,  then  links  must  be  placed  between  Ct  and  C}. 
Ihis  is  so  because  the  existence  of  c  guarantees  dial  it  is  possible  to  pass  from  any  configuration  in  C, 
to  any  in  C3  and  viceversa  while  remaining  outside  all  the  obstacles  in  COS,  and  COSj.  flic  resulting 
composite  FSG  is  then  searched  for  a  padi,  since  each  path  through  the  graph  corresponds  to  a  class 
of  safe  paths  in  Cspace,\  and  vice-versa. 


7.  Path  Searching 


The  Findpath  problem  is  to  find  a  padi  between  two  points,  the  start  and  the  goal,  while  staying  in 
die  free  space.  In  the  current  implementation,  this  is  carried  out  by  die  following  steps: 

1.  Choose  die  largest  EMPTY  cell  in  any  of  the  SR,  enclosing  die  start  configuration. 
Otherwise,  choose  some  MIXED  cell  containing  the  start  and  expand  the  representation  of 
diis  MIXED  cell  into  its  constituent  EMPTY,  MIXED  and/or  FUEL  cells.  If  an  EMPIY  cell 
contains  the  start  configuration,  stop,  else  repeat.  Note  that  this  computes  successively  finer 
models,  i.c.  successive  covering  node  sets,  of  the  specific  area  around  the  start  without  having 
to  expand  the  complete  model  or  even  any  complete  part  tree.  If  no  EMPTY  cell  is  ever  found, 
die  task  is  impossible  since  the  start  configuration  causes  a  collision. 

2.  Perform  step  1  for  die  goal  configuration. 

3.  Construct  a  Free  Space  Graph  as  described  in  Section  6.2.  At  this  point,  the  Free  Space 
Graph  is  in  its  final  form;  the  current  implementation  does  not  refine  the  space  representation 
further. 


l0Thc  current  representation  allows  EMPTY  cells  to  overlap  each  other  but  not  MIXED  or  FULL  cell*. 


I 
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4.  Search  for  die  shortest  padi  in  the  l-'rce  Space  Graph  from  the  cell  including  the  start  to 
that  including  the  goal.  Hie  graph  search  operation  can  be  carried  out  by  any  of  the  standard 
shortest  path  algorithms  [13]:  the  current  implementation  uses  the  A *  algorithm  [15].  These 
shortest  path  algorithms  require  that  a  weight  be  assigned  to  each  of  the  links  of  the  F'rce  Space 
Graph  e  g.  indicating  the  time  required  to  traverse  die  cells.  How  this  may  be  done  is  discussed 
below.  If  no  path  exists,  diis  may  be  due  to  die  approximations  and  quantizations  used  in  the 
solution,  see  Section  7.3. 

5.  Choose  a  line  path  contained  in  die  cell  path.  This  problem  is  discussed  in  Section  7.2. 


7.  /.  Assigning  link  Weights  for  the  I'SG 

The  definition  of  an  "optimal"  path,  or  even  a  "good"  path,  assumes  some  choice  of  performance 
index.  The  current  implementation  uses  estimated  time  of  travel  along  die  padi  as  die  index.  If 
Cspace ..j  is  the  manipulator’s  joint  space,  then  die  lime  to  travel  between  two  configurations  can  be 
estimated  as  the  maximum  time  for  any  of  the  joints  to  travel,  at  the  maximum  rated  joint  velocity, 
between  the  joint  sellings  at  each  configuration.  The  weights  assigned  to  die  links  in  die  FSG  should 
therefore  reflect  the  time  needed  to  travel  between  two  overlapping  cells  along  the  optimal  path.  Of 
course,  no  weight  assignment  can  actually  do  diis  since  it  requires  knowing  die  complete  optimal  path. 

A  simple  alternative  is  to  assign  to  a  link  die  estimated  time  of  travel  between  die  centroids  of  the 
cells  that  it  connects,  Iliis  weighting  function  has  the  advantage  of  being  very  easy  to  compute.  For 
small  cells  it  provides  a  good  approximation  of  die  actual  time  to  traverse  die  cells,  but  for  larger  cells 
it  might  overestimate  or  underestimate  the  actual  time,  see  Figure  10.  The  current  implcmcntadon 
uses  the  centroid  weighting  function,  but  does  not  divide  the  large  EMPTY  rectangular  cells  into 
smaller  cells;  this  will  be  implemented  in  the  near  future. 

A  more  complex  weighting  function,  which  would  typically  produce  faster  paths,  is  die  following: 
The  weight  on  die  link  between  cell  C  and  C’  is  assigned  the  time  to  traverse  C  from  p,  die  point 
of  entry  to  C,  to  p',  the  point  of  entry  into  C.  The  point  p'  is  the  one  on  C  ft  C  that  minimizes 
die  distance"  to  the  line  between  p  and  the  goal.  The  initial  C  is  the  cell  that  contains  the  start 
"Actually,  (he  difference  in  time  between  the  straight  line  path  and  one  going  through  this  point 
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h'ifparc  U.  Two  dimensional  illustration  of  railings  of  the  centroid  weighting  Function,  (a)  Overestimating 
when  one  cell  is  large,  (b)  underestimating  because  oF  limited  connectivity,  and  (c)  overestimating  because  or 
large  overlap  Ihc  solid  line  is  the  optimal  path  between  cells,  the  dashed  lines  is  the  path  that  the  function 
would  use  to  evaluate  the  distance  between  cells. 


configuration  and  the  initial  p  is  the  start  configuration.  Clearly,  this  technique  requires  much  more 
computation  that  the  centroid  weighting  described  above.  For  most  applications,  the  simpler  centroid 
function,  together  with  cell  splitting  should  suffice. 


7.2.  Choosing  a  Line  Palh 

The  search  of  the  FSG  produces  a  list  of  EMPTY  C space  a  cells  that  touch  or  overlap;  U  Is  still 
necessary  to  choose  a  specific  path,  i.c.  some  curve,  within  these  ceils.  The  simplest  type  of  path 
to  choose  is  a  piecewise  linear  one,  although  the  cells  simply  place  configuration  constraints  on  the 
manipulator  along  the  path  and  any  path  satisfying  those  constraints  will  be  safe. 

If  toe  centroid  weighting  has  been  used  for  toe  links,  it  is  natural  to  choose  a  piecewise  linear 
path  that  traverses  toe  centroids  of  toe  cells.  Of  course,  toe  straight  line  path  between  two  centroids  is 
not  guaranteed  to  remain  within  the  cells  and  might  therefore  not  be  safe.  Therefore  an  intermediate 
configuration  in  the  intersection  between  adjacent  cells  should  be  chosen.  The  centroid  of  the  inter 
section  of  adjacent  cells  on  toe  path  can  be  used  for  this  purpose;  this  is  the  technique  used  in  the 
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current  implementation.  Alternatively,  this  point  could  be  chosen  so  as  to  minimize  the  deviation 
from  a  straight  line  path  between  the  centroids.  If  the  cell  size  is  small  enough,  such  paths  are 
adequate  for  most  tasks. 

The  more  complex  weighting  scheme  described  earlier  produces  a  sequence  of  entry  points  into 
the  cells  which  may  be  connected  directly  to  obtain  a  path.  Since  the  points  arc  contained  in  the 
intersection  of  the  cells,  a  straight  line  connecting  them  is  guaranted  to  be  in  the  cell. 


7.3.  Dealing  with  Path  Search  Failure 

If  the  path  search  algorithm  fails  to  find  a  safe  path,  the  reason  for  failure  could  be  one  of  the 
following: 

1.  No  safe  paths  exist. 

2.  No  safe  paths  exist  at  the  quantization  of  orientations  chosen. 

3.  The  approximations  of  objects  by  bounding  rectangular  solids  has  removed  necessary 
maneuvering  space. 

Ihc  last  two  causes  of  failure  may  be  overcome  by  decreasing  the  orientation  quantization  and/or 
increasing  the  representation  detail  in  the  space  representation,  both  at  the  expense  of  extra  computa¬ 
tion.  'Ihis  suggests  the  possibility  of  increasing  the  accuracy  of  the  space  representation  when  a  path 
search  failure  ocurrs.  The  current  implementation  docs  not  exploit  this  possibility. 


8.  Examples 

This  section  presents  output  from  the  implementation  running  on  a  simple  example.  The  results 
are  collected  in  Figure  11. 

a.  The  initial  and  final  configuration  of  the  model,  including  the  manipulator  model.  Note 
that  the  manipulator  must  rotate  to  execute  this  motion. 


Figure  11a 

The  start  and  goal  configurations  and  the  world  model 
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Figure  lib 

START  COS:  The  Cspace  obstacles  for  the  manipulator  in  the 
start  configuration. 
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Figure  11c 

GOAL  COS:  The  Cspace  obstacles  for  the  manipulator  in  the 
goal  configuration. 
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Figure  lid 

The  Cspace  obstacles  for  the  swept  volume  of  the  manipulator 
over  a  range  of  configurations  of  the  wrist. 
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Figure  lie 

The  Cell  Path  with  superimpose 
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b.  The  COS  fur  the  start  configuration,  l-ach  convex  solid  in  die  figure  is  a  representation  of 
COx\‘~(Bj).  Note  that  most  of  these  C space  (  obstacles  arc  rectangular  solids,  except  for  those 
arising  from  the  interaction  of  the  hand,  A|.  with  block  B\  and  the  fingers.  A\  and  A2,  with 
the  table.  In  these  eases,  the  manipulator  is  so  close  to  these  obstacles  that  its  configuration  is 
inside  tire  hounding  rectangular  solid  for  the  configuration  obstacles  (In  practice,  the  sides  of 
the  bounding  rectangular  solid  arc  displaced  outward  by  some  small  t).  This  condition  causes  a 
detailed  expansion  to  be  carried  out. 

c.  The  COS  for  the  goal  configuration.  In  the  goal  configuration  none  of  the  obstacles  needs 
to  be  expanded  in  detail. 

d.  T  he  COS  for  one  of  the  intermediate  configuration  ranges.  This  COS  is  defined  for  the 
manipulator's  swept  volume  over  a  range  of  orientations  of  the  wrist  and  hand.  One  bounding 
rectangular  solid.  A),  approximates  the  swept  volume  of  the  hand  and  fingers,  A(  U  A2  U  A3. 
I  hc  solids  A4  and  A5  remain  unchanged. 

e.  The  cell  path  and  the  line  path.  This  shows  the  cells  from  the  various  space  representations 
that  compose  the  cell  path.  One  group  of  cells  correspond  to  free  space  for  the  initial 
configuration,  one  large  cell  comes  from  the  intermediate  configuration  (where  the  hand  rota¬ 
tion  takes  place),  and  the  last  group  of  cells  correspond  to  the  final  configuration.  The  line 
path  shown  goes  through  the  centroid  of  each  of  the  cells  and  also  through  the  centroids  of 
the  intersection  of  adjacent  cells  on  the  path.  Notice  that  because  the  cells  are  large,  this  path 
strategy  produces  paths  that  move  too  far  from  the  obstacles.  This  could  be  overcome  by  sub¬ 
dividing  the  cells  before  finding  die  line  path. 

f.  The  cell  path  superimposed  on  the  start  COS.  This  shows  the  relative  placing  of  the  free 
cells  relative  to  the  obstacles. 


9.  Choosing  Crusp  Configurations 


lhe  preceding  sections  have  discussed  the  problem  of  finding  safe  paths  for  the  manipulator;  this 
is  only  part  of  the  "pick  and  place”  synthesis  problem,  lhe  major  remaining  problem  is  choosing 
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a  grasp  configuration  on  the  part,  P.  For  simple  parts  and  non-clultcred  environments,  grasping  is 
amenable  to  simple  ad-hoc  solutions.  As  a  step  in  die  solution  of  this  problem,  we  deal  here  with 
choosing  grasping  configurations  for  relatively  simple  parts  in  cluttered  environments.  In  this  section, 
a  C'space  approach  to  this  problem  is  described,  although  no  implementation  of  this  approach  to 
grasping  currently  exists. 

The  grasping  problem  is  related  to  die  Findspacc  problem  introduced  in  Section  3,  insofar  as  it 
involves  choosing  a  safe  configuration  among  a  set  of  obstacles.  Hut,  there  arc  additional  constraints 
on  the  choice,  for  example: 

1.  the  manipulator’s  fingers  must  be  in  contact  with  P, 

2.  the  configuration  must  be  reachable,  and 

3.  P  must  be  stable  in  the  manipulator's  hand,  i.c.  it  will  not  slip  in  die  hand  during  a  motion. 

Ihc  first  two  conditions,  contact  and  reachability,  reflect  additional  geometric  constraints  on  the  solu¬ 
tion  to  the  l-'indspacc  problem.  The  third  condition,  stability,  reflects  aspects  of  grasping  beyond  the 
purely  geometric.  Stability  will  be  briefly  discussed  later  in  the  section. 

'Ihc  approach  to  grasping  described  here  is  based  on  die  one  described  in  [25]  and  [26],  The 
basic  idea  is  to  build  an  explicit  description  of  the  set  of  configurations  of  die  manipulator  A  for 
which  the  inside  of  the  manipulator's  fingers  arc  in  contact  with  specified  surfaces  of  P.  This  set 
of  configurations  is  some  subset  of  CO/\(P),  cal!  it  G.  Feasible  grasp  configurations  arc  those  in 
G,  diat  do  not  cause  any  collisions  with  other  objects  in  the  workspace,  i.c.  that  arc  outside  all  of 
the  COa[Bj).  In  diis  section,  the  details  of  this  approach  are  discussed.  We  make  die  following 
simplifying  assumptions: 

1 .  I  hc  manipulator  is  cartesian  and  its  hand  is  a  parallel  jaw,  i.c.  two  parallel  fingers  that  move 

along  their  common  normal. 

2.  Only  parallel  planar  surfaces,  whose  distance  from  each  other  is  less  than  the  maximum 

finger  opening,  arc  candidates  for  grasping.  These  are  known  as  grasp  surfaces. 

These  assumptions  simplify  the  method  for  identifying  feasible  grasp  configurations,  while  suggesting 
its  usefulness  and  providing  the  foundation  for  a  more  general  approach. 
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Figure  12.  The  definitions  of  F\.  F}.  Ft,  Fj.  and  //  used  in  choosing  grasping  configurations. 

9.  /.  Feasible  Grasp  C onfigurations 

Let  Pi  and  P}  be  the  parallel  faces12  of  P  to  be  grasped,  and  Fi  and  F2  be  the  inside  faces  of 
the  manipulator’s  fingers.  Figure  12.  Under  the  two  assumptions  stated  above,  when  A  grasps  P,  F\ 
and  P2  are  coplanar  with  P,  and  Pj  respectively.  Under  these  conditions,  the  legal  (x,  y,  z)  positions 
of  tva  are  restricted  to  some  plane  H  that  is  parallel  to  P,  and  Pj.  Let  GU(P„  Pj)  be  the  set  of 
configurations  of  A  for  which  tva  is  in  H  and  for  which  P„  Pj,  F\,  and  P2  arc  mutually  parallel.  Note 
that  GA(P„  Pj)  represents  those  positions  and  orientations  where  A  could  be  when  grasping  P,  and 
Pj,  without  specifying  the  distance  between  the  fingers.  GA[Pi,  Pj)  is  called  the  grasp  set  for  Pf-  and 

Note  that  not  all  the  configurations  in  GA(Pt,  Pj)  arc  feasible  grasp  configurations,  either  because 

l2Notc  that  objects  in  the  current  implementation  arc  modelled  as  unions  of  convex  polyhcdra.  Convex  pdyhedra  are 
defined  as  the  intersection  of  a  finite  number  of  half-spaces,  where  each  half-space  is  bounded  by  a  plane.  The  portion 
of  each  bounding  plane  on  the  boundary  of  the  polyhedron  is  a  convex  polygon,  known  as  a  fact  of  the  object 


A 
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the  fingers  arc  not  in  contact  with  the  grasp  surfaces  or  because  the  manipulator  configuration  causes  a 
collision  with  some  other  object.  Therefore,  we  must  impose  two  additional  restrictions: 

1.  Ilte  internal  faces  of  the  fingers  must  overlap  the  grasp  surfaces. 

2.  The  manipulator  must  not  collide  with  any  other  object  in  the  workspace,  i.e.  the  By 

With  these  restrictions  on  the  configurations  in  the  grasp  set.  we  obtain  the  set  of  feasible  grasp 
configurations,  called  a  feasible  grasp  sei  and  denoted  FGA(Pi,  P}). 

Define  the  configurations  of  F|  and  Fi  to  correspond  to  those  of  the  manipulator,  i.e.  each 
position  and  orientation  of  these  faces  is  characterized  by  the  manipulator  configuration  which  would 
place  them  there.  From  dicse  definitions  it  follows  that  COr,(Pi)  is  die  set  of  those  configurations  of 
A  for  which  the  F\  is  in  contact  with  Pt.  Furdicrmore.  COyfP,)  f)G.\(P,,  P})  arc  those  configurations 
for  which  die  finger  is  in  surface-surface  contact  with  P,.  therefore,  it  follows  that 


FG,(P„  P,)  =  (co/t(P,)  n  coFi{Pj)  n  ga(p„  p,))  -  JJccub,) 

i 

In  diis  definition,  we  must  let  P  be  one  of  the  By  say  Bp,  so  as  to  avoid  collisions  with  P  while 
approaching  a  grasp  configuration,  but  we  must  also  allow  A  to  contact  P  on  die  grasp  surfaces.  The 
answer  is  to  add  a  slight  displacement  inward  to  P,  and  Pj,  when  computing  CO\(Bp),  while  using 
the  original  definition  in  the  computation  of  CO/.-,{Pi)  and  COyfPj). 

'Flic  feasible  grasp  set,  as  defined  above,  is  a  volume  in  a  six-dimensional  CspaceA.  We  do  not 
have  algorithms  for  computing  this  volume  exactly.  T  he  algoridims  of  Section  3  serve  only  to  com¬ 
pute  slice  projections  of  die  C  space  A  obstacles.  It  is  clear  that  die  same  must  be  done  for  the  feasible 
grasp  set,  namely  computing  its  slice  projection  for  some  range  of  orientations.  Such  a  slice  would  be 
the  set  of  (i,  y,  z)  positions  of  A  that,  for  some  range  of  orientations  of  A,  arc  in  contact  with  P,  but 
outside  all  of  the  Bj.  Presumably,  this  requires  using  the  slice  projections  of  COyfPi),  COyfPj),  and 
the  COa(Bj).  A  problem  arises  when  trying  to  do  this,  because  slice  projections  were  defined  over 
simple  orientation  ranges  of  the  cartesian  manipulator's  wrist  defined  in  Section  5.  these  ranges  are 
not,  in  general  compatible  with  the  ranges  of  orientations  that  define  GA(Pi,  Pj).  For  a  posidon  of 
rvA  on  H,  only  a  small  range  of  orientations  will  result  in  configurations  that  arc  in  GA(P{,  Pj),  yet  for 
that  position  to  be  in  a  slice  of  FGA(P„  Pj)  it  must  be  the  ease  that  no  orientation  within  the  slice’s 
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defining  range  causes  a  collision.  Ihcrcforc,  few,  if  any,  configurations  in  the  grasp  set  will  be  feasible 
grasp  configurations. 

The  solution  to  this  problem  is  simply  to  define  a  new  set  of  slices  w  hose  orientation  ranges  are 
subsets  of  the  orientation  ranges  in  G  \(P„  Pf).  Note  that  a  configuration  in  such  a  slice  already 
satisfies  the  orientation  constraints  of  the  grasp  set.  Therefore,  only  the  position  constraints,  i.c.  that 
the  (x,  y,z)  position  be  in  H,  need  to  be  enforced  to  obtain  the  intersection  of  a  C space  obstacle  in 
that  slice  with  the  grasp  set.  This  removes  the  need  of  computing  the  complete  representation  of  the 
obstacles,  while  simultaneously  avoiding  the  problems  introduced  by  irrelevant  orientations. 

Computing  the  obstacle  slices  for  orientations  in  the  grasp  set  requires  being  able  to  compute  the 
swept  volume  of  the  manipulator  over  orientation  ranges  that  are  not  the  simple  ranges  of  joint  angles 
defined  in  Section  5.  I.ct  R  be  the  set  of  orientations  in  the  grasp  set  that  define  a  slice  and  denote 
the  swept  volume  of  A  over/f  as  A  [ft],  Algorithms  for  approximating  the  swept  volume  over  these 
ranges  can  be  based  on  the  simple  approach  described  in  the  Appendix.  The  impot  w.nt  constraint  on 
the  approximation  to  A  [ft]  is  that  it  docs  not  intersect  the  grasp  s  trfaccs  for  positions  of  rvA  on  H. 

In  addition  to  the  manipulator  displacing  and  rotating,  the  manipulator’s  fingers  may  move  per¬ 
pendicular  to  the  grasp  surfaces.  This  additional  degree  of  freedom  has  not  been  discussed  above.  In 
fact,  it  poses  no  additional  problems;  the  motion  of  the  fingers  can  be  treated,  via  slice  projection, 
uniformly  with  rotation.  This  simply  requires  including  the  space  swept  out  by  the  fingers  during 
closing,  in  the  swept  volume  used  to  define  slices  of  the  COA(B0). 


9.2.  Overlap  of  Finger  arid  Surface 

The  approach  described  above  deals  adequately  with  the  CO.\(Bj )  in  the  definition  of  feasible 
grasp  set,  but  is  less  succcsfu!  in  dealing  with  CO/.-^P,)  and  COrfPj).  The  reason  for  this  is  that 
a  position  in  the  slice  projection  of  CO/  , (Pi)  simply  indicates  that  for  some  orientation  of  A  in  the 
slice,  the  finger  is  in  contact  with  P,.  What  is  required  instead  is  the  set  of  positions  which  for  all 
orientations  of  A  in  the  slice,  there  is  contact  In  fact,  we  would  like  to  guarantee  that  the  area  of 
contact  between  the  fingers  and  the  grasp  faces  always  exceeds  some  fixed  area.  How  this  may  be 
accomplished  is  discussed  below. 


At 
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Figure  13.  Defining  the  configurations  of  A  for  which  /•'  overlaps  (a)  Illustration  of  the  definition  of  7* 
and  S.  (b)  Illustration  of  f  05.  with  two  positions  of  /•*' s  reference  vertex  (indicated  by  the  small 

circles)  showing  the  area  of  overlap  includes  an  area  of  the  form  Tk  0  s,  for  some  »6S. 


Let  Fk  and  Pi  be,  respectively,  a  finger  surface  and  the  corresponding  grasp  surface.  We  define 
Tk  to  be  a  small  strip  at  the  tip  of  Fk,  such  that  Fk  =  Tk  0  S,  where  S  is  the  set  of  points  along  a 
line  segment,  as  shown  in  Figure  13.  Again,  we  assume  that  the  configurations  of  7*  correspond  to 
those  of  Fk  (and  therefore  A).  Assume  A  is  in  some  configuration  c  E  GA{Pt,  Pj),  so  that  Fk  and  Pi 
arc  coplanar,  then  CFjF(Pi)  is  the  set  of  (x,  y,z)  configurations  of  7*.  and  therefore  of  F*  and  A, 
for  which  Fk  0  Pi  D  Tk.  But,  we  do  not  want  to  restrict  the  overlap  between  Fk  and  Pi  to  be  at  the 
fingertip;  instead,  wc  want  the  area  of  overlap  to  include  some  area  T'k,  obtainable  by  translating  7* 
along  S,  i.e.  T’k  =  Tk  ®  {«},  with  a  E  5.  It  is  easy  to  show  that 
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CI%*(Pi)  ©  S  =  {  c  I  3«  e  S  :  P,  n  (Fk)c  D  (Tfc)c  ©  {a}  > 

Therefore,  this  is  the  desired  set  of  configurations,  see  Figure  13.  This  result  can  be  applied  to  com¬ 
pute  the  slices  needed  for  the  feasible  grasp  set.  If  R  is  the  orientation  range  defining  the  slice,  then 
CI^IP,)  ©  5[ft]  represents  the  set  of  (x,  y,  z]  configurations  that,  for  orientations  in  R,  guarantee 
that  the  contact  between  Fk  and  Pi  includes  Tk.  Note  that  this  approach  can  be  gcncrali/.cd  to  any  S 
and  Tk  such  that/7'*  =  7V®  S;  as  Tk  becomes  smaller  and  approaches  a  point,  then  S  approaches Fk. 


9.3.  Safely  at  the  Destination 

So  far,  the  definition  of  FGA(Pi,  Pj)  only  embodies  constraints  relating  to  safety  at  P’s  initial 
configuration,  however  a  grasp  configuration  must  also  be  safe  at  P’s  final  configuration.  Clearly, 
another  feasible  grasp  set  can  be  computed  at  P’s  final  configuration,  say  FGA{P'i,  P' )  where  the 
primed  faces  indicate  the  faces  at  their  final  configuration.  But,  these  two  feasible  grasp  sets  cannot 
be  intersected  to  obtain  those  grasp  configurations  that  are  safe  for  both  configurations  of  P,  be¬ 
cause  a  grasp  configuration  corresponds  to  different  manipulator  configurations  at  each  different 
configuration  of  P.  What  is  needed  is  a  way  of  defining  those  grasp  configurations  in  P’s  initial 
configuration  that  would  lead  to  a  collision  when  P  is  in  its  final  configuration.  Figure  14. 

A  grasp  configuration  establishes  a  fixed  relationship  between  the  fingers  and  the  grasped  part, 
P.  Let  the  final  configuration  of  P  be  obtained  by  a  displaccmnct  consisting  of  a  translation  t  and  a 
rotation  r,  indicated  by  Dlr(P).  Clearly,  any  set  of  positions  X  bears  the  same  relationship  to  DtiT(P) 
as  Dfrl[X)  bears  to  P.  Therefore,  if  CO%iz(Bj)  is  a  set  of  positions  of  A  which  cause  collisions  at  P’s 
final  configuration,  then  DfJ(CO*?*{Bj))  represent  infeasible  grasp  configurations.  Figure  14.  litis 
result  also  holds  for  swept  volumes  of  A,  therefore  it  may  be  used  to  ensure  safety  at  the  destination  in 
the  definition  of  feasible  grasp  sets. 

9.4.  Computing  the  Feasible  Grasp  Set 

The  discussion  in  the  preceding  subsections  is  summarized  in  the  following  definition  of  feasible 
grasp  set,  for  some  range  of  orientation  in  the  grasp  set.  We  denote  this  orientation  range  as  R,  and 
let  R!  denote  the  same  orientation  range  as  R  relative  to  P,  but  at  P’s  destination.  We  also  let  ((,  r)  be 


Figure  14.  (a)  A  side  view  of  a  manipulator  hand,  composed  of  a  finger  and  a  "palm",  hold¬ 
ing  F  al  the  initial  and  final  configuration,  (b)  In  the  initial  configuration,  the  shaded  area 
represents  COj^P.)  —  Q  CO*Ay,{Bj).  i.c.  the  feasible  grasp  configurations  for  A,  considering 
only  safety  at  the  origin  and  letting  T  be  a  point,  (c)  'Ihe  CO\v*{Bj)  for  the  final  configuration 
of  A  and  P.  (d)  1110  shaded  area  represents  CO^*(P,)  —  u  D-,(COJv*(Bj)), 

wh»ch  is  the  feasible  grasp  set  that  takes  into  account  safety  at  the  destination. 
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the  displacement  between  the  initial  and  final  configurations  of  P.  lhen,  the  feasible  grasp  set,  for  the 
orientation  range  R  and  displacement  ( t ,  r),  is: 

FG*aUP'>  *»)  =  ((ClfrUP>)  n  CI^n](P:))  ©  S[H})  -  \J  CO^Bj)  U  D^CO^Bj)) 

3 

All  of  the  elements  in  this  definition  can  be  computed  using  the  COI,Jt  algorithm  of  Section  3.2  and  a 
swept  volume  algorithm. 


9.5.  Approach  and  Departure 

Configurations  in  the  feasible  grasp  set,  as  defined  above,  arc  guaranteed  to  be  safe  both  at  P's 
initial  and  final  configuration.  While  these  conditions  arc  sufficient  in  most  situations,  they  do  not 
guarantee  that  the  feasible  grasp  configurations  can  be  used  during  a  "pick  and  place"  operation.  For 
a  feasible  grasp  configuration  to  be  a  legal  grasp  configuration,  it  must  allow  the  manipulator  to  reach 
and  depart  P’s  initial  and  final  configurations.  Summarizing,  the  following  conditions  must  hold  for  a 
legal  grasp  configuration: 

1.  It  must  be  possible  to  reach  it  from  the  initial  configuration  of  the  manipulator. 

2.  It  must  be  possible  to  remove  P  from  its  initial  configuration  safely. 

3.  It  must  be  possible  to  reach  the  P’s  final  configuration  with  P  held  in  the  hand. 

4.  It  must  be  possible  to  withdraw  the  manipulator  from  P’s  final  configuration. 

The  Findpath  algorithm  described  in  the  proceeding  sections  can  be  extended  to  deal  with 
the  problem  of  choosing  a  grasping  configuration  that  is  reachable  from  the  manipulator’s  initial 
configuration.  As  we  saw  above,  the  feasible  grasp  configurations,  over  some  range  of  orientations,  are 
those  within  some  specified  volume  of  C  space  a,  but  outside  the  slice  projections  of  suitably  defined 
C space A  obstacles.  Hence,  they  are  equivalent  to  the  slices,  COS\c,  <f\  of  Section  6.2.  Therefore, 
a  free  space  representation  for  the  feasible  grasp  configurations  can  be  constructed  and  the  resulting 
free  cells  linked  in  the  Free  Space  Graph.  The  feasible  grasp  configurations  for  alternative  grasp 
surfaces  can  also  be  linked  into  the  graph.  In  the  resulting  FSG,  any  path  from  die  cell  containing 
the  origin  to  a  cell  containing  a  feasible  grasp  configuration  shows  that  this  grasp  configuration  may 
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be  reached  from  the  origin.  The  path  searching  process  must  be  modified  to  search  for  any  cell 
which  contains  a  suitable  grasp  configuration,  rather  than  searching  for  a  particular  cell  containing  the 
destination. 

Similarly,  departure  from  the  origin  and  approach  to  the  destination  could  be  handled  by  testing 
whether  the  destination  is  reachable,  using  the  TSG  constructed  as  above,  lhc  difference  is  that  now 
die  hand  is  holding  P,  therefore  the  polyhedral  description  of  P  must  be  treated  as  if  it  were  part  of 
die  manipulator.  This  requires  adding  a  new  set  of  C space  \  obstacles,  arising  from  the  interaction  of 
P  and  the  objects  in  the  workspace,  to  die  ones  already  computed  for  the  manipulator.  This  is  entirely 
analogous  to  modifying  the  description  of  the  manipulator,  which  is  already  modelled  as  a  union  of 
convex  solids,  but.  the  geometric  relationships  between  P  and  die  A*  arc  determined  by  the  grasp 
configuration,  which  has  several  degrees  of  freedom.  The  problem  can  be  approached  by  treating 
these  additional  degrees  of  freedom,  via  slice  projection,  just  as  the  wrist  rotations  were  treated.  This 
approach  imposes  a  great  cost  in  additional  computation.  A  simpler,  though  less  general,  technique  is 
to  use  heuristics  in  choosing  a  feasible  grasp  configuration  and  then  test,  via  the  path  search  process, 
whether  that  grasp  configuration  permits  departure.  If  it  does  not,  a  new  configuration  might  be 
chosen  and  the  process  repeated.  This  approach  would  be  not  be  adequate  for  very  cluttered  environ¬ 
ments  or  situations  involving  parts  mating  at  the  destination.  In  such  environments  an  approach  based 
on  slice  projection  would  also  be  susceptible  to  failure.  Further  research  is  needed  in  diis  area. 


9.6.  Stability  in  Grasping 

We  have  thus  far  not  considered  the  issue  of  stability  of  the  feasible  grasp  point.  An  adequate 
treatment  of  stability  in  grasping  is  not  yet  available,  although  some  promising  approaches  exist  [5]. 
The  techniques  described  in  this  section  can  be  used  to  implement  two  simple  grasping  heuristics, 
which  work  adequately  when  (1)  the  manipulator  hand  is  made  up  of  rigid  fingers,  (2)  die  object  to  be 
grasped,  P,  is  small  relative  to  the  manipulator  hand  and  (3)  parts  mating  effects  arc  ignored.  The  two 
heuristics  arc: 

1.  [insure  at  least  a  minimum  contact  area  of  the  fingers  with  the  grasp  surfaces.  lhc  amount 
of  overlap  should  depend  on  object  properties  such  as  weight  and  surface  smoothness. 


38 


The  l-flcci  of  Uncertainty 


2.  Ilic  perpendicular  projection  of  P’s  center  of  mass  should  be  near  to  F|  O  Pi  and  P2  0  P>. 

I'hc  implementation  of  the  contact  area  heuristic  was  discussed  above,  Section  9.2.  The  center 
of  mass  heuristic  can  be  implemented  by  giving  preference  to  grasp  surfaces  for  which  the  center  of 
mass,  projected  onto  the  plane  containing  P„  falls  within  P,  and  similarly  for  Pj.  Furthermore,  for 
specified  grasp  surfaces,  the  choice  among  legal  grasp  configurations  should  minimize  the  distance  of 
the  prjcction  of  the  center  of  mass  to  the  area  of  overlap  between  finger  and  grasp  surface. 

131056  heuristics,  though  adequate  for  many  tasks,  are  not  a  substitute  for  a  general  theory  of 
stability  in  grasping.  This  remains  one  of  the  most  interesting  open  problems  in  robotics. 


10.  'I'hc  Effect  of  Uncertainty 

In  the  proceeding  sections  we  have  assumed  that: 

1.  the  configuration  of  all  the  objects  is  known  exactly,  and 

2.  the  configuration  of  the  manipulator  can  be  controlled  exactly . 

Both  of  these  assumptions  are  only  approximations  to  reality.  In  practice,  configurations  can  only 
be  known  to  within  some  uncertainty.  Both  of  these  sources  of  uncertainty  aflfcct  what  manipulator 
motions  are  safe. 


10. 1.  Modelling  Worst-Case  Uncertainly  in  C apace, \ 

In  Capoce/t,  the  two  sources  of  uncertainty  have  similar  effects,  i.c.  modifying  the  shape  of  the 
CspaceA  obstacles.  This  section  deals  with  techniques  for  taking  into  account  these  effects.  The 
following  notation  is  useful  in  the  discussion.  Let  e  =  (/?,)  =  ((J\, . . .  ,/?„)  E  Rn  and  similarly,  let 
configurations  be  (nr*)  =  hi, •  ■■,'ln)  E  Rn-  The  index  set  {1, . . . ,n}  will  be  referred  to  as  /;  let 
K  C I.  The  set  Un{e)  denotes  the  set  of  configurations  in  CspaceA  whose  /('-parameters  arc  less  than 
the  absolute  value  of  the  corresponding  parameter  ofe. 


hi)  €  v,<{c)  «=> 


f— Pi  <  1 i  <  Pi* 

V*  =  0. 


ift  e  K 
otherwise 
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Uncertainty  in  the  configuration  of  A  in  CspaceA  can  be  represented  as  a  region  around  its 
nominal  configuration,  c\  within  this  region  arc  all  the  configurations  that  A  may  be  in.  Simple  regions 
can  be  characterized  by  {c}  ©  U^(eA).  Assume  that  (A)Q  nfi  ^  0,  i.c  that  a  £  COA(B).  Any 
nominal  configuration  a!  such  that  a'  -f-  x  =  a,  for  x  £  UA (e.(),  should  also  belong  to  COA(B).  Ihis 
means  that  under  uncertainty  of  A.  COA(B)  should  be  replaced  with  COfiB)  ©  U/%(eA).  In  practice, 
we  do  not  ever  compute  CO  \(B)\  rather,  we  compute  slice  projections  of  it  using  the  swept  volume 
of  A  over  ranges  of  orientation  parameters,  R.  Therefore  the  orientation  and  translation  uncertainty 
must  be  treated  separately.  Orientation  uncertainty  affects  the  definition  of  the  manipulator’s  swept 
volume.  For  example,  to  compute  a  slice  with  parameters  (c,  e']/<,  the  swept  volume  A\c—eA,  d-\-eA]n 
is  used  in  place  of  A[c,  d\n.  The  effect  of  the  uncertainty  in  the  translation  parameters,  T,  can  be 
computed  as  indicated  in  Section  5.1.  using  the  C0\,IZ(B)  algorithm. 

Ihe  worst-case  effect  on  COa(Bj)  of  uncertainty  in  the  configuration  of  die  BJt  can  be  modelled 
by  replacing  B  with  the  swept  volume  of  B  over  die  uncertainty  range.  Alternatively,  if  the  uncer¬ 
tainty  in  the  configuration  of  B}  can  be  approximated  by  an  uncertainty  in  translation13  ,  Ut^b,)  then 
the  uncertainty  of  A  and  B  can  be  combined  into  a  single  uncertainty1 1  and  treated  as  the  uncertainty 
of  A.  If  T  is  the  set  of  indices  for  translation  parameters,  then  the  combined  uncertainty  is: 


tWO  =  ut(*b,)  ©  Vr(cA) 


10.2.  The  Effect  of  Uncertainly  on  "Pick  and  Place"  Synthesis 

Ihe  presence  of  uncertainty  significantly  affects  manipulator  programming  in  general  and  the 
synthesis  of  "pick  and  place"  motions  in  particular.  One  approach  to  planning  motions  in  the 
presence  of  uncertainty  is  to  plan  paths  that  arc  safe  under  die  worst  case  uncertainty,  i.e.  paths 
outside  the  expanded  CspaceA  obstacles  defined  above.  This  approach  rules  out  most  operations 
that  involve  moving  near  objects,  c.g.  grasping.  Another  approach  is  to  assume  that  uncertainty 
docs  not  significantly  affect  the  outcome  of  most  operations  and  to  plan  motions  assuming  nominal 

l3This  can  be  done  by  defining  a  new  translation  uncertainly  such  that  the  swept  volume  over  this  range  of  positions 
will  contain  (he  swept  volume  over  the  original  unccitainty  range. 

Mfhis  assumes  that  the  translation  space  of  the  manipulator  is  the  same  ns  that  of  the  objects  in  the  workspace,  which 
is  true  for  cartesian  manipulators. 


40 


I  he  I  fl'cci  of  Uncertainty 


configurations.  A  compromise  position  is  to  redefine  the  "pick  and  place"  synthesis  problem  so  as  to 
isolate  those  operations  that  arc  most  susceptible  to  uncertainty  from  those  others  where  uncertainty 
plays  a  relatively  minor  role.  The  latter  can  be  addressed  by  the  techniques  outlined  in  this  paper,  the 
former  require  a  different  approach.  One  possible  re-definition  of  the  "pick  and  place"  problem  is  tire 
following: 

1.  Kind  a  nominal  grasp  configuration  assuming  that  there  is  no  uncertainty. 

2.  Identify  a  grasp  approach  configuration,  a  configuration  that  can  be  shown  to  be  safe  under 
worst-ease  uncertainty  estimates  for  object  and  manipulator  configuration. 

3.  Identify  a  grasp  dcproach  configuration,  a  configuration  which  is  safe  for  the  manipulator 
grasping  the  part,  given  the  uncertainty  in  the  part's  configuration  after  grasping  and  the  uncer¬ 
tainty  in  configurations  of  nearby  objects. 

4.  Compute  a  path,  from  the  manipulator’s  initial  configuration  to  the  grasp  approach 
configuration,  assuming  worst-ease  uncertainty. 

5.  Identify  a  destination  approach  configuration,  a  configuration  which  is  safe  for  the  manipulator 
holding  the  object,  given  the  uncertainty  in  the  grasp  configuration  and  the  uncertainly  of 
nearby  objects. 

6.  Compute  a  safe  path  from  the  grasp  dcproach  configuration  to  the  destination  approach 
configuration  for  die  manipulator  and  the  grasped  part,  also  assuming  wort-ease  uncertainty. 

7.  Identify  a  destination  depproach  configuration,  a  configuration  which  is  safe  for  the 
manipulator,  given  the  uncertainty  of  nearby  objects. 

This  formulation  of  the  synthesis  problem  factors  out  the  problems  of  approaching  and  dcproach- 
ing  both  the  nominal  grasp  configuration  and  the  destination.  For  both  of  these  problems,  the  use 
of  sensory  information  to  identify  the  actual  state  of  the  task  and  to  accomodate  to  it  is  important 
[25]  [30]  [44].  When  the  uncertainty  is  small,  the  problem  can  be  dealt  with  by  ad  hoc  methods, 
c.g.  opening  the  fingers  very  wide  and  relying  on  the  grasping  action  to  place  the  object  and/or 
the  manipulator  in  approximately  the  correct  orientation  [19],  the  general  problem  of  planning 
manipulator  operations  that  arc  robust  in  the  face  of  uncertainty  is  an  important  problem  ['*01,  but 


41 


Summary 


beyond  the  scope  of  this  paper. 


11.  Summary 

This  paper  has  presented  an  approach  to  the  central  geometric  problems  underlying  the  synthesis 
of  "pick  and  place"  motions  for  cartesian  manipulators.  The  key  technique  in  the  approach  is  the  use 
of  explicit  polyhedral  representations  of  the  configuration  constraints  on  the  manipulator.  This  repre¬ 
sentation  permits  the  use  of  simple  and  powerful  geometric  operations  to  solve  problems  involving 
safe  motions  of  the  manipulator,  in  particular,  the  problems  of  finding  grasp  configurations  and  safe 
paths  in  the  absence  of  uncertainty. 

The  concepts  of  Configuration  Space  and  Configuration  Space  Obstacle  have  played  a  central  role 
in  the  approach  to  gross  motion  synthesis  developed  here.  Similar  concepts  play  an  important  role  in 
the  approach  to  compliant  motion  synthesis  described  in  [30],  These  concepts  have  also  proven  useful 
in  other  geometric  applications  (1]  |2]  [4]  [45]. 
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figure  15.  Ihe  WEDGE  is  a  convex  polyhedron  used  to  approximate  the  volume  swept  out  by  a  cuboid 
aligned  with  the  coordinate  axes,  as  it  rotates  around  the  z  axis,  assuming  the  z  axis  docs  not  penetrate  the 
cuboid. 


Appendix  1.  A  Polyhedral  Approximation  for  Swept  Volume 


The  swept  volume  is  the  volume  occupied  by  a  polyhedron  over  a  set  of  configurations,  c.g.  along 
some  path.  The  swept  volume  over  a  range  of  translations  can  be  computed  using  the  CO*1'*  algo¬ 
rithm.  In  this  appendix,  we  will  limit  our  attention  to  computing  a  simple  polyhedral  approximation 
to  the  swept  volume  for  rotations  of  a  polyhedron  around  an  arbitrary  axis.  This  method  is  included 
here  for  completeness,  it  is  not  the  best  polyhedral  approximation  to  the  swept  volume. 

The  swept  volume  approximation  described  here  returns  a  list  of  convex  polyhedra  of  two  types: 

I.  CYLINDHR  —  a  polyhedral  approximation  to  a  right  circular  cylinder. 
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1  WEDGE  —  a  polyhedral  approximation  to  the  volume  swept  out  by  a  cuboid,  aligned  with 
the  coordinate  axes,  as  it  rotates  around  the  z  axis,  Figure  15.  It  assumes  that  the  z  axis  does 
not  penetrate  the  cuboid  and  that  the  rotation  is  less  than  t. 

The  input  is  a  polyhedron,  0.  an  axis  of  rotation  which  is  the  z  axis  of  a  reference  frame  and  0, 
the  angle  of  rotation.  The  first  step  is  to  rotate  the  frame  around  z  so  that  the  x  axis  goes  through 
the  centroid  of  the  projection  of  B  on  the  (x,  y)-planc  of  the  frame.  Compute  an  aligned  bounding 
rectangular  solid  for  0,  00(0).  whose  dimensions  arc  (Ax,  A y,  Ax).  If  the  z  axis  docs  not  pass 
through  the  object,  then  if  $  <  0lnax  <  r  then  simply  return  a  WEDGE  enclosing  the  swept  volume. 
If  the  z  axis  penetrates  00(0),  then  if  Ax  >  Ay.  cut  0  using  the  planes  x—  ^  and  z  —  =fik, 
and  return  a  cylinder  of  radius  x/2  Ay  whose  height  is  A z  and  return  the  swept  volumes  of  the  pieces 
of  0  beyond  the  central  area.  The  procedure  is  similar  if  Ay  >  Ax.  Figure  16  illustrates  this 
process.  Here  0mnx  is  some  user  specified  parameter,  although  it  could  be  chosen  to  guarantee  some 
kind  of  error  bound.  If  0  >  0„mx,  then  divide  the  rotation  into  a  set  of  successive  rotations  each 
returning  a  wedge. 


